#!/usr/bin/env python3
# coding: utf-8

import os
import threading
import time
import yaml
import json
import re

import rospy
from std_msgs.msg import String
from std_msgs.msg import Bool
from geometry_msgs.msg import PoseStamped
from geometry_msgs.msg import Twist
from actionlib_msgs.msg import GoalStatusArray
import actionlib
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal


class SequentialNavigator(object):
	def __init__(self):
		rospy.init_node('sequential_nav_detect', anonymous=False)

		# Params
		self.frame_id = rospy.get_param('~frame_id', 'map')
		# Accept multiple possible param names for the yaml path
		self.goals_yaml = rospy.get_param('~goals_yaml', None)
		if self.goals_yaml is None:
			self.goals_yaml = rospy.get_param('~goals_yaml_path', None)
		if self.goals_yaml is None:
			self.goals_yaml = rospy.get_param('~nav_goal_yaml', None)
		if not self.goals_yaml:
			rospy.logerr('未提供~goals_yaml参数（nav_goal.yaml路径）。请通过launch传入。')
			raise rospy.ROSInitException('goals_yaml 参数缺失')

		if not os.path.isabs(self.goals_yaml):
			self.goals_yaml = os.path.abspath(self.goals_yaml)

		if not os.path.exists(self.goals_yaml):
			rospy.logerr('goals_yaml 文件不存在: {}'.format(self.goals_yaml))
			raise rospy.ROSInitException('goals_yaml 文件不存在')

		# Timeouts (seconds)
		self.nav_timeout_sec = float(rospy.get_param('~nav_timeout', 300.0))
		self.detect_timeout_sec = float(rospy.get_param('~detect_timeout', 120.0))
		self.kickball_time_limit_sec = float(rospy.get_param('~kickball_time_limit', 70.0))
		self.global_time_limit_sec = float(rospy.get_param('~global_time_limit_sec', 170.0))

		# Action client for move_base
		self.mb_client = actionlib.SimpleActionClient('/move_base', MoveBaseAction)
		if not self.mb_client.wait_for_server(rospy.Duration(5.0)):
			rospy.logerr('等待 /move_base ActionServer 超时，请确认 move_base 已启动')
			raise rospy.ROSInitException('move_base action server 不可用')

		# Publishers
		self.goal_pub = rospy.Publisher('/move_base_simple/goal', PoseStamped, queue_size=10, latch=True)
		self.yolo_pub = rospy.Publisher('/yolo_detect', String, queue_size=10)
		# /nx_nano: 用于接收 'next'/'end' 指令，也用于我们发布 'start ...' 或 'end'
		self.nx_nano_pub = rospy.Publisher('/nx_nano', String, queue_size=10)
		self.start_nav_pub = rospy.Publisher('/nx_nano/start_nav', Bool, queue_size=10, latch=True)
		self.cmd_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=10)

		# Internal state
		self._last_goal_sent_time = rospy.Time(0)
		self._nav_succeeded = threading.Event()
		self._detected = threading.Event()
		self._kick_cmd_event = threading.Event()
		self._kick_cmd = None
		# yolo 结果缓存
		self._yolo_results_raw = []
		self._yolo_parsed = []  # 列表项: (color:str, state:str)
		self._yolo_parsed_recent = []  # 本阶段缓存
		self._yolo_parsed_main = []  # one..six 阶段缓存
		self._in_main_phase = True  # one..six 阶段标识
		self._yolo_raw_main = []  # one..six 阶段收到的原始文本（过滤 'yolostart'）
		self._yolo_buffer_start_time = None
		self._start_enter_time = None  # 回车开始时刻
		self._selected_colors = ('red', 'blue')
		self.allowed_colors = set(['red', 'orange', 'yellow', 'blue', 'green', 'purple'])

		# Subscribers
		self.status_sub = rospy.Subscriber('/move_base/status', GoalStatusArray, self._on_status)
		# Primary topic per requirement
		self.detect_sub = rospy.Subscriber('/dog_action_statet', String, self._on_detect)
		# Backward compatibility with existing controller that publishes '/dog_action_state'
		self.detect_sub_compat = rospy.Subscriber('/dog_action_state', String, self._on_detect)
		# Kickball flow control -> 改为 /nx_nano
		self.kickball_nav_sub = rospy.Subscriber('/nx_nano', String, self._on_kickball_nav)
		# 订阅 yolo_result 结果（用于统计颜色）
		self.yolo_result_sub = rospy.Subscriber('/yolo_result', String, self._on_yolo_result)

		# Load goals
		self.goals = self._load_goals(self.goals_yaml)
		if not self.goals:
			rospy.logerr('未能从 {} 读取到 one..six 的目标点'.format(self.goals_yaml))
			raise rospy.ROSInitException('未找到有效目标点')

		# Optional extra goal: navigate to 'box1' after 'six' without triggering detection
		self.box1_goal = self._load_named_goal(self.goals_yaml, 'box1')
		# Optional intermediate goals: ball1..ball4 between 'six' and 'box1'
		self.ball_goals = self._load_ball_goals(self.goals_yaml)

		rospy.loginfo('顺序导航节点启动，共 {} 个目标点'.format(len(self.goals)))

	def _load_goals(self, yaml_path):
		with open(yaml_path, 'r', encoding='utf-8') as f:
			data = yaml.safe_load(f) or {}
		goals_ns = data.get('goals', {})
		sequence_keys = ['one', 'two', 'three', 'four', 'five', 'six']
		seq = []
		for key in sequence_keys:
			g = goals_ns.get(key)
			if not g:
				rospy.logwarn('在 {} 中未找到目标: {}'.format(yaml_path, key))
				continue
			pos = g.get('position', None)
			ori = g.get('orientation', None)
			if not (isinstance(pos, (list, tuple)) and len(pos) == 3 and isinstance(ori, (list, tuple)) and len(ori) == 4):
				rospy.logwarn('目标 {} 的 position 或 orientation 格式不正确'.format(key))
				continue
			seq.append({'name': key, 'position': pos, 'orientation': ori})
		return seq

	def _load_named_goal(self, yaml_path, goal_name):
		with open(yaml_path, 'r', encoding='utf-8') as f:
			data = yaml.safe_load(f) or {}
		goals_ns = data.get('goals', {})
		g = goals_ns.get(goal_name)
		if not g:
			return None
		pos = g.get('position', None)
		ori = g.get('orientation', None)
		if not (isinstance(pos, (list, tuple)) and len(pos) == 3 and isinstance(ori, (list, tuple)) and len(ori) == 4):
			rospy.logwarn('目标 {} 的 position 或 orientation 格式不正确'.format(goal_name))
			return None
		return {'name': goal_name, 'position': pos, 'orientation': ori}

	def _load_ball_goals(self, yaml_path):
		"""按顺序加载可用的 ball1..ball4 目标点。缺失的将被忽略。"""
		with open(yaml_path, 'r', encoding='utf-8') as f:
			data = yaml.safe_load(f) or {}
		goals_ns = data.get('goals', {})
		ball_names = ['ball1', 'ball2', 'ball3', 'ball4']
		balls = []
		for name in ball_names:
			g = goals_ns.get(name)
			if not g:
				continue
			pos = g.get('position', None)
			ori = g.get('orientation', None)
			if not (isinstance(pos, (list, tuple)) and len(pos) == 3 and isinstance(ori, (list, tuple)) and len(ori) == 4):
				rospy.logwarn('目标 {} 的 position 或 orientation 格式不正确'.format(name))
				continue
			balls.append({'name': name, 'position': pos, 'orientation': ori})
		return balls

	def _on_status(self, msg):
		# 不再依赖该回调判断成功
		return

	def _on_detect(self, msg):
		# 收到任意消息即视为识别成功
		self._detected.set()

	def _on_yolo_result(self, msg):
		"""订阅 yolo_result 的结果，保存并打印。

		可能收到我们自己发送的 'yolostart'，需要忽略统计。
		支持解析包含 color/state 的结构化文本或两段式文本。
		"""
		try:
			text = str(msg.data).strip()
		except Exception:
			text = ''
		if not text:
			return
		# 打印原始结果
		rospy.loginfo("收到 yolo_result 消息: {}".format(text))
		self._yolo_results_raw.append(text)
		# 忽略触发命令
		if text.lower() == 'yolostart':
			return
		# six 之前的主阶段：保存原始文本（过滤 'yolostart'）
		if self._in_main_phase:
			self._yolo_raw_main.append(text)
		# 尝试解析 color/state
		color = None
		state = None
		lower_text = text.lower()
		# 1) 解析 JSON/YAML
		try:
			obj = yaml.safe_load(text)
			if isinstance(obj, dict):
				c = obj.get('color')
				s = obj.get('state')
				if isinstance(c, str):
					color = c.strip().lower()
				if isinstance(s, str):
					state = s.strip().lower()
		except Exception:
			pass
		# 2) 正则匹配 color/state=xxx
		if color is None or state is None:
			m = re.search(r"color\s*[:=]\s*([a-zA-Z_]+)", lower_text)
			if m:
				color = m.group(1)
			m = re.search(r"state\s*[:=]\s*([a-zA-Z_]+)", lower_text)
			if m:
				state = m.group(1)
		# 3) 两段式: "<color> <state>" 或 "," 分隔
		if (color is None or state is None) and ((' ' in lower_text) or (',' in lower_text) or ('-' in lower_text)):
			seps = ['-', ',', ';']
			temp = lower_text
			for sp in seps:
				temp = temp.replace(sp, ' ')
			parts = [p for p in temp.split() if p]
			if len(parts) >= 3:
				# 常见三段式: <color> <index> <state>
				color = parts[0]
				state = parts[-1]
			elif len(parts) >= 2:
				# 退化为两段式，前 color 后 state
				color = parts[0]
				state = parts[1]
		if isinstance(color, str) and isinstance(state, str):
			self._yolo_parsed.append((color, state))
			self._yolo_parsed_recent.append((color, state))
			# six 之前的主阶段收集
			if self._in_main_phase:
				self._yolo_parsed_main.append((color, state))
			rospy.loginfo("解析到 yolo 结果: color={}, state={}".format(color, state))

	def _log_main_phase_yolo_summary(self):
		"""在 six->ball1 过渡前打印 one..six 阶段收到/解析到的 YOLO 结果与异常频次统计。"""
		try:
			raw_count = len(self._yolo_raw_main)
			parsed_count = len(self._yolo_parsed_main)
			rospy.loginfo("six->ball1 前调试：one..six 阶段收到的 yolo_result 原始结果(过滤 'yolostart') 共 {} 条:".format(raw_count))
			rospy.loginfo(str(self._yolo_raw_main))
			rospy.loginfo("six->ball1 前调试：one..six 阶段成功解析的 (color, state) 共 {} 条:".format(parsed_count))
			rospy.loginfo(str(self._yolo_parsed_main))
			# 统计 up/down 频次
			counts_updown = {}
			for color, state in self._yolo_parsed_main:
				if color not in self.allowed_colors:
					continue
				st = (state or '').strip().lower()
				if st in ('up', 'down'):
					counts_updown[color] = counts_updown.get(color, 0) + 1
			sorted_updown = sorted(counts_updown.items(), key=lambda kv: (-kv[1], kv[0]))
			counts_updown_str = ", ".join(["{}:{}".format(c, n) for c, n in sorted_updown]) if sorted_updown else "(无 up/down 结果)"
			rospy.loginfo("six->ball1 前调试：up/down 颜色频次：{}".format(counts_updown_str))
		except Exception as e:
			rospy.logwarn("six->ball1 前调试日志输出异常: {}".format(e))

	def _clear_yolo_buffer(self):
		self._yolo_results_raw = []
		self._yolo_parsed_recent = []
		self._yolo_buffer_start_time = time.time()

	def _get_top_two_colors(self):
		counts = {}
		for color, state in self._yolo_parsed_recent:
			if color not in self.allowed_colors:
				continue
			counts[color] = counts.get(color, 0) + 1
		if not counts:
			# 回退默认
			return ('red', 'blue')
		sorted_colors = sorted(counts.items(), key=lambda kv: (-kv[1], kv[0]))
		if len(sorted_colors) == 1:
			return (sorted_colors[0][0], sorted_colors[0][0])
		return (sorted_colors[0][0], sorted_colors[1][0])

	def _get_top_two_abnormal_colors_main(self):
		"""在 one..six 收集阶段中，基于 state!="normal" 的异常统计颜色频次，取前两名。

		返回: (c1, c2)
		"""
		counts = {}
		for color, state in self._yolo_parsed_main:
			if color not in self.allowed_colors:
				continue
			st = (state or '').strip().lower()
			if st != 'normal':
				counts[color] = counts.get(color, 0) + 1
		if not counts:
			return ('red', 'blue')
		sorted_colors = sorted(counts.items(), key=lambda kv: (-kv[1], kv[0]))
		if len(sorted_colors) == 1:
			return (sorted_colors[0][0], sorted_colors[0][0])
		return (sorted_colors[0][0], sorted_colors[1][0])

	def _get_top_two_updown_colors_main(self):
		"""在 one..six 收集阶段中，只统计 state 为 'up'/'down' 的颜色频次，取前两名。

		若 one..six 阶段没有任意 up/down，则回退到 one..six 阶段所有解析到的颜色频次最高的两个颜色。
		返回: (c1, c2)
		"""
		counts = {}
		for color, state in self._yolo_parsed_main:
			if color not in self.allowed_colors:
				continue
			st = (state or '').strip().lower()
			if st in ('up', 'down'):
				counts[color] = counts.get(color, 0) + 1
		if not counts:
			# 无 up/down，使用主阶段整体出现频次最高的两个颜色（不使用固定颜色）
			all_counts = {}
			for color, state in self._yolo_parsed_main:
				if color not in self.allowed_colors:
					continue
				all_counts[color] = all_counts.get(color, 0) + 1
			if not all_counts:
				return (None, None)
			sorted_colors_all = sorted(all_counts.items(), key=lambda kv: (-kv[1], kv[0]))
			if len(sorted_colors_all) == 1:
				return (sorted_colors_all[0][0], sorted_colors_all[0][0])
			return (sorted_colors_all[0][0], sorted_colors_all[1][0])
		sorted_colors = sorted(counts.items(), key=lambda kv: (-kv[1], kv[0]))
		if len(sorted_colors) == 1:
			return (sorted_colors[0][0], sorted_colors[0][0])
		return (sorted_colors[0][0], sorted_colors[1][0])

	def _global_time_exceeded(self):
		if self._start_enter_time is None:
			return False
		return (time.time() - float(self._start_enter_time)) > float(self.global_time_limit_sec)

	def _on_kickball_nav(self, msg):
		"""接收 /nx_nano 指令，支持 'next' 与 'end' 指令（忽略 'start*'）。"""
		try:
			data = str(msg.data).strip().lower()
		except Exception:
			data = ''
		# 忽略 'start' 指令（包含颜色参数）
		if data.startswith('start'):
			return
		if data in ('next', 'end'):
			self._kick_cmd = data
			self._kick_cmd_event.set()
			rospy.loginfo("收到 /nx_nano 指令: {}".format(data))
		else:
			# 静默忽略其它字符串，避免与 'start color color' 冲突
			return

	def _wait_for_kick_cmd(self, wait_timeout=0.1):
		"""短轮询等待一条来自 /kickball_nav 的有效指令，返回 'next' 或 'end'，若超时返回 None。"""
		if self._kick_cmd_event.wait(timeout=float(wait_timeout)):
			cmd = self._kick_cmd
			self._kick_cmd = None
			self._kick_cmd_event.clear()
			return cmd
		return None

	def _publish_goal(self, position, orientation):
		# 通过 Action 发送目标，以更一致地接收结果
		goal = MoveBaseGoal()
		goal.target_pose.header.stamp = rospy.Time.now()
		goal.target_pose.header.frame_id = self.frame_id
		goal.target_pose.pose.position.x = float(position[0])
		goal.target_pose.pose.position.y = float(position[1])
		goal.target_pose.pose.position.z = float(position[2])
		goal.target_pose.pose.orientation.x = float(orientation[0])
		goal.target_pose.pose.orientation.y = float(orientation[1])
		goal.target_pose.pose.orientation.z = float(orientation[2])
		goal.target_pose.pose.orientation.w = float(orientation[3])

		self.mb_client.send_goal(goal)
		self._last_goal_sent_time = rospy.Time.now()
		self._nav_succeeded.clear()
		rospy.loginfo('已下发导航目标 ({:.3f}, {:.3f}, {:.3f})'.format(
			goal.target_pose.pose.position.x, goal.target_pose.pose.position.y, goal.target_pose.pose.position.z))

	def _wait_for_nav_success(self):
		if self.nav_timeout_sec <= 0:
			self.mb_client.wait_for_result()
		else:
			finished = self.mb_client.wait_for_result(rospy.Duration(self.nav_timeout_sec))
			if not finished:
				self.mb_client.cancel_goal()
				rospy.logwarn('导航等待超时 ({:.1f}s)，已取消目标'.format(self.nav_timeout_sec))
				return False
		state = self.mb_client.get_state()
		# SUCCEEDED == 3
		if state == 3:
			return True
		else:
			rospy.logwarn('导航未成功，状态码: {}'.format(state))
			return False

	def _trigger_yolo_and_wait(self):
		# 触发识别
		self._detected.clear()
		self.yolo_pub.publish(String(data='yolostart'))
		rospy.loginfo('已发布识别触发: yolostart 到 /yolo_detect')
		# 等待识别完成
		start = time.time()
		rate = rospy.Rate(20)
		while not rospy.is_shutdown():
			if self._detected.is_set():
				return True
			if self.detect_timeout_sec > 0 and (time.time() - start) > self.detect_timeout_sec:
				rospy.logwarn('识别等待超时 ({:.1f}s)'.format(self.detect_timeout_sec))
				return False
			rate.sleep()

	def _backoff_before_box1(self, speed_mps=1, duration_sec=1.0):
		"""在下发 box1 前，先沿 x 轴后退指定时间。

		参数:
		- speed_mps: 后退速度，单位 m/s（正数表示速度幅值，内部会取负号）
		- duration_sec: 持续时间，单位 s
		"""
		backward_twist = Twist()
		backward_twist.linear.x = -abs(float(speed_mps))
		zero_twist = Twist()
		rospy.loginfo('在下发 box1 之前，先后退 {:.2f} m/s 持续 {:.1f}s'.format(abs(float(speed_mps)), float(duration_sec)))
		start_time = time.time()
		rate = rospy.Rate(20)
		while not rospy.is_shutdown() and (time.time() - start_time) < float(duration_sec):
			self.cmd_vel_pub.publish(backward_twist)
			rate.sleep()
		# 停止
		self.cmd_vel_pub.publish(zero_twist)
		rospy.sleep(0.1)

	def _strafe_right_before_box1(self, speed_mps=0.5, duration_sec=0.5):
		"""在下发 box1 前，后退完成后再向右平移指定时间。

		坐标约定：前方为 +x，左侧为 +y，因此右移为 -y。

		参数:
		- speed_mps: 右移速度，单位 m/s（正数表示速度幅值，内部会取负号）
		- duration_sec: 持续时间，单位 s
		"""
		right_twist = Twist()
		right_twist.linear.y = -abs(float(speed_mps))
		zero_twist = Twist()
		rospy.loginfo('在下发 box1 之前，先右移 {:.2f} m/s 持续 {:.1f}s'.format(abs(float(speed_mps)), float(duration_sec)))
		start_time = time.time()
		rate = rospy.Rate(20)
		while not rospy.is_shutdown() and (time.time() - start_time) < float(duration_sec):
			self.cmd_vel_pub.publish(right_twist)
			rate.sleep()
		# 停止
		self.cmd_vel_pub.publish(zero_twist)
		rospy.sleep(0.1)

	def run(self):
		# 等待用户按回车开始，并记录全局计时起点
		try:
			_ = input('按回车开始顺序导航与识别...')
		except Exception:
			pass
		self._start_enter_time = time.time()
		rospy.loginfo('已开始，全局限时 {:.1f}s'.format(self.global_time_limit_sec))

		for idx, g in enumerate(self.goals, start=1):
			if rospy.is_shutdown():
				break
			name = g['name']
			rospy.loginfo('开始导航到第 {} 个目标: {}'.format(idx, name))
			self._publish_goal(g['position'], g['orientation'])
			succeeded = self._wait_for_nav_success()
			if succeeded:
				_ = self._trigger_yolo_and_wait()
			else:
				rospy.logwarn('导航未成功，跳过识别，进入下一个目标')
			rospy.loginfo('完成第 {} 个目标处理: {}'.format(idx, name))

		# After completing 'six', optionally traverse ball1..ball4 via kickball flow, then navigate to 'box1'
		if any(g['name'] == 'six' for g in self.goals):
			already_navigated_to_box1 = False
			if self.ball_goals:
				rospy.loginfo("已完成'six'，开始遍历 ball1..ball4 目标点（受 /kickball_nav 控制）")
				# 在 six -> ball1 之间后退与右移（0.5 m/s，0.8s）
				self._backoff_before_box1(speed_mps=0.5, duration_sec=3)
				self._strafe_right_before_box1(speed_mps=0.5, duration_sec=3)
				# 在切换阶段前先输出 one..six 阶段收到的所有 yolo 结果，便于排查
				self._log_main_phase_yolo_summary()
				# 切换阶段：结束主阶段(one..six)收集
				self._in_main_phase = False
				# 清空 YOLO 缓存，开始统计 ball1 阶段颜色
				self._clear_yolo_buffer()
				b0 = self.ball_goals[0]
				self._publish_goal(b0['position'], b0['orientation'])
				_ = self._wait_for_nav_success()
				rospy.loginfo('完成 ball 目标: {}'.format(b0['name']))
				# 全局限时检查：若超时则发布 end 并转向 box1
				if self._global_time_exceeded():
					rospy.logwarn('全局限时 {:.1f}s 已到，发布 /nx_nano: end 并转向 box1'.format(self.global_time_limit_sec))
					self.nx_nano_pub.publish(String(data='end'))
					if self.box1_goal:
						self._publish_goal(self.box1_goal['position'], self.box1_goal['orientation'])
						box1_ok = self._wait_for_nav_success()
						if box1_ok:
							rospy.loginfo("到达 box1，开始在 10.0s 内以 5Hz 发布 /nx_nano/start_nav=True")
							start_time = time.time()
							rate = rospy.Rate(5)
							msg = Bool(data=True)
							while not rospy.is_shutdown() and (time.time() - start_time) < 10.0:
								self.start_nav_pub.publish(msg)
								rospy.loginfo("已向 /nx_nano/start_nav 发布 True 触发信号")
								rate.sleep()
							rospy.loginfo("/nx_nano/start_nav 有限时发布结束")
					already_navigated_to_box1 = True
				else:
					# 使用 one..six 阶段中 state 为 up/down 的统计结果选择颜色，避免固定 red/blue
					c1, c2 = self._get_top_two_updown_colors_main()
					if c1 is None or c2 is None:
						rospy.logwarn("one..six 阶段未解析到任何可用的颜色结果，跳过发布 start c1 c2")
					else:
						self._selected_colors = (c1, c2)
						cmd_start = 'start {} {}'.format(c1, c2)
						self.nx_nano_pub.publish(String(data=cmd_start))
						rospy.loginfo("已向 /nx_nano 发布: '{}'".format(cmd_start))
				# 等待 /kickball_nav 指令来决定下一步
				next_index = 1
				while not rospy.is_shutdown():
					# 轮询等待指令，同时检查限时
					cmd = self._wait_for_kick_cmd(wait_timeout=0.1)
					# 全局限时检查：若超时则向 /nx_nano 发布 'end' 并转向 box1
					if self._global_time_exceeded():
						rospy.logwarn('全局限时 {:.1f}s 已到，发布 /nx_nano: end 并转向 box1'.format(self.global_time_limit_sec))
						self.nx_nano_pub.publish(String(data='end'))
						if self.box1_goal:
							self._publish_goal(self.box1_goal['position'], self.box1_goal['orientation'])
							box1_ok = self._wait_for_nav_success()
							if box1_ok:
								rospy.loginfo("到达 box1，开始在 10.0s 内以 5Hz 发布 /nx_nano/start_nav=True")
								start_time = time.time()
								rate = rospy.Rate(5)
								msg = Bool(data=True)
								while not rospy.is_shutdown() and (time.time() - start_time) < 10.0:
									self.start_nav_pub.publish(msg)
									rospy.loginfo("已向 /nx_nano/start_nav 发布 True 触发信号")
									rate.sleep()
								rospy.loginfo("/nx_nano/start_nav 有限时发布结束")
						already_navigated_to_box1 = True
						break
					if cmd is None:
						continue
					if cmd == 'next':
						if next_index < len(self.ball_goals):
							# 切换到下一球：清空统计并下发导航
							self._clear_yolo_buffer()
							b = self.ball_goals[next_index]
							self._publish_goal(b['position'], b['orientation'])
							_ = self._wait_for_nav_success()
							rospy.loginfo('完成 ball 目标: {}'.format(b['name']))
							# 统计颜色并下发 start color1 color2
							# 复用首次选择的颜色，不再二次统计
							c1, c2 = self._selected_colors if hasattr(self, '_selected_colors') and self._selected_colors else (None, None)
							if c1 is None or c2 is None:
								# 兜底：若首次未设置，才统计一次
								c1, c2 = self._get_top_two_colors()
								self._selected_colors = (c1, c2)
							cmd_start = 'start {} {}'.format(c1, c2)
							self.nx_nano_pub.publish(String(data=cmd_start))
							rospy.loginfo("已向 /nx_nano 发布: '{}'".format(cmd_start))
							next_index += 1
						else:
							rospy.logwarn('收到 next 指令，但已无更多 ball 目标，忽略')
					elif cmd == 'end':
						rospy.loginfo("收到 'end' 指令，直接导航到 'box1'")
						if self.box1_goal:
							self._publish_goal(self.box1_goal['position'], self.box1_goal['orientation'])
							box1_ok = self._wait_for_nav_success()
							if box1_ok:
								rospy.loginfo("到达 box1，开始在 10.0s 内以 5Hz 发布 /nx_nano/start_nav=True")
								start_time = time.time()
								rate = rospy.Rate(5)
								msg = Bool(data=True)
								while not rospy.is_shutdown() and (time.time() - start_time) < 10.0:
									self.start_nav_pub.publish(msg)
									rospy.loginfo("已向 /nx_nano/start_nav 发布 True 触发信号")
									rate.sleep()
								rospy.loginfo("/nx_nano/start_nav 有限时发布结束")
						already_navigated_to_box1 = True
						break
			# 若未通过 'end' 导航到 box1，则按原逻辑在完成 ball* 后导航到 box1（不进行后退/右移）
			if self.box1_goal and not already_navigated_to_box1:
				rospy.loginfo("追加导航到'box1'（不触发识别）")
				self._publish_goal(self.box1_goal['position'], self.box1_goal['orientation'])
				box1_ok = self._wait_for_nav_success()
				if box1_ok:
					rospy.loginfo("到达 box1，开始在 10.0s 内以 5Hz 发布 /nx_nano/start_nav=True")
					start_time = time.time()
					rate = rospy.Rate(5)
					msg = Bool(data=True)
					while not rospy.is_shutdown() and (time.time() - start_time) < 10.0:
						self.start_nav_pub.publish(msg)
						rospy.loginfo("已向 /nx_nano/start_nav 发布 True 触发信号")
						rate.sleep()
					rospy.loginfo("/nx_nano/start_nav 有限时发布结束")

		rospy.loginfo('所有目标处理完成')


def main():
	try:
		node = SequentialNavigator()
		node.run()
	except rospy.ROSInitException:
		pass
	except Exception as e:
		rospy.logerr('异常: {}'.format(e))


if __name__ == '__main__':
	main()


